
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.DigitalIOButton;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.*;


/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OperatorInterface {
    private static final int PITCH_ENABLE_BUTTON = 11;

    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);
    
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());

    Joystick DriverJoystickLeft = new Joystick(RobotMap.DRIVER_JOYSTICK_LEFT);
    Joystick DriverJoystickRight = new Joystick(RobotMap.DRIVER_JOYSTICK_RIGHT);
    Joystick ShooterJoystick = new Joystick(RobotMap.SHOOTER_JOYSTICK);
    Joystick ProFlightJoystick = new Joystick(RobotMap.PRO_FLIGHT_JOYSTICK);
    Joystick CustomControlInterface = new Joystick(RobotMap.CUSTOM_CONTROL_INTERFACE);
    Joystick DriverJoystickDualAction = new Joystick(RobotMap.DRIVER_JOYSTICK_DUAL_ACTION);
    
    Button buttonL1 = new JoystickButton(DriverJoystickLeft,1);
    Button buttonL2 = new JoystickButton(DriverJoystickLeft,2);
    Button buttonL3 = new JoystickButton(DriverJoystickLeft,3);
    Button buttonL4 = new JoystickButton(DriverJoystickLeft,4);
    Button buttonL5 = new JoystickButton(DriverJoystickLeft,5);
    Button buttonL6 = new JoystickButton(DriverJoystickLeft,6);
    Button buttonL7 = new JoystickButton(DriverJoystickLeft,7);
    Button buttonL8 = new JoystickButton(DriverJoystickLeft,8);
    Button buttonL9 = new JoystickButton(DriverJoystickLeft,9);
    Button buttonL10 = new JoystickButton(DriverJoystickLeft,10);
    Button buttonL11 = new JoystickButton(DriverJoystickLeft,11);

    Button buttonR1 = new JoystickButton(DriverJoystickRight,1);
    Button buttonR2 = new JoystickButton(DriverJoystickRight,2);
    Button buttonR3 = new JoystickButton(DriverJoystickRight,3);
    Button buttonR4 = new JoystickButton(DriverJoystickRight,4);
    Button buttonR5 = new JoystickButton(DriverJoystickRight,5);
    Button buttonR6 = new JoystickButton(DriverJoystickRight,6);
    Button buttonR7 = new JoystickButton(DriverJoystickRight,7);
    Button buttonR8 = new JoystickButton(DriverJoystickRight,8);
    Button buttonR9 = new JoystickButton(DriverJoystickRight,9);
    Button buttonR10 = new JoystickButton(DriverJoystickRight,10);
    Button buttonR11 = new JoystickButton(DriverJoystickRight,11);

    Button buttonS1 = new JoystickButton(ShooterJoystick,1);
    Button buttonS2 = new JoystickButton(ShooterJoystick,2);
    Button buttonS3 = new JoystickButton(ShooterJoystick,3);
    Button buttonS4 = new JoystickButton(ShooterJoystick,4);
    Button buttonS5 = new JoystickButton(ShooterJoystick,5);
    Button buttonS6 = new JoystickButton(ShooterJoystick,6);
    Button buttonS7 = new JoystickButton(ShooterJoystick,7);
    Button buttonS8 = new JoystickButton(ShooterJoystick,8);
    Button buttonS9 = new JoystickButton(ShooterJoystick,9);
    Button buttonS10 = new JoystickButton(ShooterJoystick,10);
    Button buttonS11 = new JoystickButton(ShooterJoystick,11);
    Button buttonS12 = new JoystickButton(ShooterJoystick,12);
    
//    Button buttonP1 = new JoystickButton(ProFlightJoystick,1);
//    Button buttonP2 = new JoystickButton(ProFlightJoystick,2);
//    Button buttonP3 = new JoystickButton(ProFlightJoystick,3);
//    Button buttonP4 = new JoystickButton(ProFlightJoystick,4);
//    Button buttonP5 = new JoystickButton(ProFlightJoystick,5);
//    Button buttonP6 = new JoystickButton(ProFlightJoystick,6);
//    Button buttonP7 = new JoystickButton(ProFlightJoystick,7);
//    Button buttonP8 = new JoystickButton(ProFlightJoystick,8);
    
    //unused
    Button buttonC1 = new JoystickButton(CustomControlInterface,1);
    Button buttonC2 = new JoystickButton(CustomControlInterface,2);
    Button buttonC3 = new JoystickButton(CustomControlInterface,3);
    Button buttonC4 = new JoystickButton(CustomControlInterface,4);
    Button buttonC5 = new JoystickButton(CustomControlInterface,5);
    Button buttonC6 = new JoystickButton(CustomControlInterface,6);
    
    //unused
    Button buttonD1 = new JoystickButton(DriverJoystickDualAction,1);
    Button buttonD2 = new JoystickButton(DriverJoystickDualAction,2);
    Button buttonD3 = new JoystickButton(DriverJoystickDualAction,3);
    Button buttonD4 = new JoystickButton(DriverJoystickDualAction,4);
    Button buttonD5 = new JoystickButton(DriverJoystickDualAction,5);
    Button buttonD6 = new JoystickButton(DriverJoystickDualAction,6);
    Button buttonD7 = new JoystickButton(DriverJoystickDualAction,7);
    Button buttonD8 = new JoystickButton(DriverJoystickDualAction,8);
    Button buttonD9 = new JoystickButton(DriverJoystickDualAction,9);
    Button buttonD10 = new JoystickButton(DriverJoystickDualAction,10);
    
    
   public OperatorInterface(){
       
         //Driver left joystick button allocations
         buttonL1.whenPressed(new DoHighGear());
         //button 2 is free
         buttonL3.whenPressed(new DoClimberDeploy());
         //buttons 4-9 is free
         buttonL6.whenPressed(new DoSetLEDState(true));
         buttonL7.whenPressed(new DoSetLEDState(false));

         //Driver right joystick button allocations
         buttonR1.whenPressed(new DoLowGear());
         //button 2 is free
         buttonR3.whenPressed(new DoClimberRetract());
         //buttons 4-9 is free
         buttonR10.whenPressed(new DoSetPhotonCannonState(false));
         buttonR11.whenPressed(new DoSetPhotonCannonState(true));
         
         //Shooter joystick button allocations
         buttonS1.whenPressed(new DoShoot());
         buttonS2.whileHeld(new DoCollectorForward());
         buttonS2.whenReleased(new DoCollectorStop());
         //button 3 is free
         //button 4 is free
         buttonS5.whileHeld(new DoCollectorReverse());
         buttonS5.whenReleased(new DoCollectorStop());
         //button 6 is free
         buttonS7.whileHeld(new DoCollectorSetPositionSpeed(-0.50));
         buttonS7.whenReleased(new DoCollectorSetPositionSpeed(0.0));
         buttonS8.whileHeld(new DoCollectorSetPositionSpeed(0.50));
         buttonS8.whenReleased(new DoCollectorSetPositionSpeed(0.0));
         //buttons 9 and 10 are free 
         //button 11 is what enables the pitch 
         //button 12 is free
         
         //ProFlight joystick button allocations
//         buttonP1.whileHeld(new DoRunHopper());
//         
          //Custom control interface for the shooter button allocations
         buttonC1.whenPressed(new DoShoot());
         buttonC2.whenPressed(new DoCollectorForward());
         buttonC2.whenReleased(new DoCollectorStop());
         buttonC3.whileHeld(new DoCollectorReverse());
         buttonC4.whenPressed(new DoCollectorSetPositionSpeed(0.50));
         buttonC5.whenPressed(new DoCollectorSetPositionSpeed(-0.50));
         buttonC6.whenPressed(new DoSetPhotonCannonState(true));
         buttonC6.whenReleased(new DoSetPhotonCannonState(false));
         
         //Driver dual action controller button allocations
         buttonD1.whenPressed(new DoSetPhotonCannonState(true));
         buttonD2.whenPressed(new DoSetPhotonCannonState(false));
         //button 3 is free
         //button 4 is free
         buttonD5.whenPressed(new DoHighGear());
         buttonD6.whenPressed(new DoLowGear());
         buttonD7.whenPressed(new DoClimberDeploy());
         buttonD8.whenPressed(new DoClimberRetract());
         buttonD9.whenPressed(new DoSetLEDState(true));
         buttonD10.whenPressed(new DoSetLEDState(false));
       
    }   
    public double GetDriverJoystickLeftMove(){
        return -DriverJoystickLeft.getY();
    }
    
    public double GetDriverJoystickRightMove(){
        return -DriverJoystickRight.getY();
    }
    
    public double GetShooterJoysitckZ(){
        return (-ShooterJoystick.getThrottle()+1.0)/2;
    }
    
    public double GetDriverLeftJoysitckZ(){
        return (-DriverJoystickLeft.getZ()+1.0)/2;
    }
    
    public double GetDriverRightJoysitckZ(){
        return (-DriverJoystickRight.getZ()+1.0)/2;
    }
    
    public double GetShooterJoystickX(){
        return ShooterJoystick.getX();
    }
    
    public double GetShooterJoystickY() {
        return(ShooterJoystick.getY());
    }
    
    public double GetShooterJoystickTwist() {
        return(ShooterJoystick.getTwist());
    }
    
//    public double GetProFlightJoystickX() {
//        return(ProFlightJoystick.getX());
//    }
//    public double GetProFlightJoystickY() {
//        return(ProFlightJoystick.getY());
//    }
//    
//    public double GetProFlightJoystickZ() {
//        return(ProFlightJoystick.getZ());
//    }
    
    public double GetDriverJoystickDualActionLeftMove() {
        return DriverJoystickDualAction.getRawAxis(2);
    }
   
   public double GetDriverJoystickDualActionRightMove() {
        return DriverJoystickDualAction.getRawAxis(4);
    }
    
    public boolean ShooterPitchEnabled() {
        return ShooterJoystick.getRawButton(PITCH_ENABLE_BUTTON);
    }
    
    public double GetProFlightDelay() {
        return ((-ProFlightJoystick.getX()+1.0)/2) * 5;
    }
//    
//    public double GetCollectorSpeed() {
//        return (-ProFlightJoystick.getZ());
//    }
    
    public double GetShooterPitchTime() {
        return 0.90;
    }
    
//    public double GetProFlightDistance() {
//        return ((-ProFlightJoystick.getX()+1.0)/2) * 15;
//    }
    
    public double GetFeederTimeout() {
        return 3.0;
    }
    
    public double GetPickupTimeout() {
        return 0.0;
    }
}
